﻿/*
 * ©2009-2011 XELF
 * http://xelf.info
 */

#if XNA || SILVERLIGHT || WINDOWS_PHONE
using Microsoft.Xna.Framework;
using _Quaternion = Microsoft.Xna.Framework.Quaternion;
using QuaternionHelper = XELF.Framework.QuaternionHelper;
#else
using _Quaternion = XELF.Framework.Quaternion;
using QuaternionHelper = XELF.Framework.Quaternion;
#endif

namespace XELF.Framework {

	/// <summary>
	/// デュアルクォータニオン
	/// </summary>
	public partial struct DualQuaternion {
		/// <summary>
		/// 実部
		/// </summary>
		public Quaternion Real;
		/// <summary>
		/// 非実部
		/// </summary>
		public Quaternion Dual;
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// コンストラクタ
		/// </summary>
		/// <param name="rx"></param>
		/// <param name="ry"></param>
		/// <param name="rz"></param>
		/// <param name="rw"></param>
		/// <param name="dx"></param>
		/// <param name="dy"></param>
		/// <param name="dz"></param>
		/// <param name="dw"></param>
		public DualQuaternion(float rx, float ry, float rz, float rw, float dx, float dy, float dz, float dw) {
			Real = new Quaternion(rx, ry, rz, rw);
			Dual = new Quaternion(dx, dy, dz, dw);
		}
		/// <summary>
		/// コンストラクタ
		/// </summary>
		/// <param name="real"></param>
		/// <param name="dual"></param>
		public DualQuaternion(Quaternion real, Quaternion dual) {
			Real = real;
			Dual = dual;
		}
	}

	public partial struct DualQuaternion {
		public static bool AreEqual(ref DualQuaternion a, ref DualQuaternion b, float delta) {
			return
				QuaternionHelper.AreEqual(ref a.Real, ref b.Real, delta) &&
				QuaternionHelper.AreEqual(ref a.Dual, ref b.Dual, delta);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 逆デュアルクォータニオン。Conjugate(a) / LengthSquared(a)
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void Invert(ref DualQuaternion a, out DualQuaternion result) {
			float rr, rd;
			a.ReciprocalLengthSquared(out rr, out rd);
			result = new DualQuaternion(
				-a.Real.X * rr,
				-a.Real.Y * rr,
				-a.Real.Z * rr,
				+a.Real.W * rr,
				-a.Dual.X * rr - a.Real.X * rd,
				-a.Dual.Y * rr - a.Real.Y * rd,
				-a.Dual.Z * rr - a.Real.Z * rd,
				+a.Dual.W * rr + a.Real.W * rd);
		}
		/// <summary>
		/// 逆デュアルクォータニオン
		/// </summary>
		/// <param name="a"></param>
		/// <returns></returns>
		public static DualQuaternion Invert(DualQuaternion a) {
			DualQuaternion result;
			Invert(ref a, out result);
			return result;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 文字列表現を得る。
		/// </summary>
		/// <returns></returns>
		public override string ToString() {
			return string.Format(System.Globalization.CultureInfo.InvariantCulture,
				"[{0}]+ɛ[{1}]", Real, Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 回転と位置からデュアルクォータニオン
		/// </summary>
		/// <param name="rotation"></param>
		/// <param name="position"></param>
		/// <returns></returns>
		public static DualQuaternion CreateFromRotationPosition(Quaternion rotation, Vector3 position) {
			DualQuaternion result;
			CreateFromRotationPosition(ref rotation, ref position, out result);
			return result;
		}
		/// <summary>
		/// 回転と位置からデュアルクォータニオン
		/// </summary>
		/// <param name="rotation"></param>
		/// <param name="position"></param>
		/// <param name="result"></param>
		public static void CreateFromRotationPosition(ref Quaternion rotation, ref Vector3 position,
			out DualQuaternion result) {
			result = new DualQuaternion(rotation, new Quaternion(
				0.5f * (position.X * rotation.W + position.Y * rotation.Z - position.Z * rotation.Y),
				0.5f * (-position.X * rotation.Z + position.Y * rotation.W + position.Z * rotation.X),
				0.5f * (position.X * rotation.Y - position.Y * rotation.X + position.Z * rotation.W),
				-0.5f * (position.X * rotation.X + position.Y * rotation.Y + position.Z * rotation.Z)));
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 回転を作る（回転クォータニオンからデュアルクォータニオン）
		/// </summary>
		/// <param name="rotation"></param>
		/// <param name="result"></param>
		public static void CreateRotation(ref Quaternion rotation,
			out DualQuaternion result) {
			result = new DualQuaternion(rotation, new Quaternion());
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 平行移動を作る（位置ベクトルからデュアルクォータニオン）
		/// </summary>
		/// <param name="translation"></param>
		/// <param name="result"></param>
		public static void CreateTranslation(ref Vector3 translation,
			out DualQuaternion result) {
			result = new DualQuaternion(Quaternion.Identity,
				new Quaternion(translation.X * 0.5f, translation.Y * 0.5f, translation.Z * 0.5f, 0));
		}
		public static DualQuaternion CreateTranslation(Vector3 translation) {
			DualQuaternion result;
			CreateTranslation(ref translation, out result);
			return result;
		}
		public Vector3 Position {
			get {
				var ll = Real.LengthSquared();
				var r = 1 / ll;
				float w = Real.W, x = Real.X, y = Real.Y, z = Real.Z;
				float w2 = w + w, x2 = x + x, y2 = y + y, z2 = z + z;
				float a = Dual.W, b = Dual.X, c = Dual.Y, d = Dual.Z;

				var result = new Vector3(
					-a * x2 + w2 * b - c * z2 + y2 * d,
					-a * y2 + b * z2 - x2 * d + w2 * c,
					-a * z2 + x2 * c + w2 * d - b * y2);
				Vector3.Multiply(ref result, r, out result);
				return result;
			}
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 和
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Add(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			Quaternion.Add(ref a.Real, ref b.Real, out result.Real);
			Quaternion.Add(ref a.Dual, ref b.Dual, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 差
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Subtract(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			Quaternion.Subtract(ref a.Real, ref b.Real, out result.Real);
			Quaternion.Subtract(ref a.Dual, ref b.Dual, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a">元のデュアルクォータニオン</param>
		/// <param name="scale">スカラー</param>
		/// <param name="result">結果</param>
		public static void Multiply(ref DualQuaternion a, float scale, out DualQuaternion result) {
			Quaternion.Multiply(ref a.Real, scale, out result.Real);
			Quaternion.Multiply(ref a.Dual, scale, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 線形補間
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="amount"></param>
		/// <param name="result"></param>
		public static void Lerp(ref DualQuaternion a, ref DualQuaternion b, float amount, out DualQuaternion result) {
			DualQuaternion c = new DualQuaternion();
			Quaternion.Subtract(ref b.Real, ref a.Real, out c.Real);
			Quaternion.Subtract(ref b.Dual, ref a.Dual, out c.Dual);
			Quaternion.Multiply(ref c.Real, amount, out c.Real);
			Quaternion.Multiply(ref c.Dual, amount, out c.Dual);
			Quaternion.Add(ref c.Real, ref a.Real, out result.Real);
			Quaternion.Add(ref c.Dual, ref a.Dual, out result.Dual);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Multiply(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			result = new DualQuaternion(
				a.Real.W * b.Real.X + a.Real.X * b.Real.W + a.Real.Y * b.Real.Z - a.Real.Z * b.Real.Y,
				a.Real.W * b.Real.Y - a.Real.X * b.Real.Z + a.Real.Y * b.Real.W + a.Real.Z * b.Real.X,
				a.Real.W * b.Real.Z + a.Real.X * b.Real.Y - a.Real.Y * b.Real.X + a.Real.Z * b.Real.W,
				a.Real.W * b.Real.W - a.Real.X * b.Real.X - a.Real.Y * b.Real.Y - a.Real.Z * b.Real.Z,
				a.Real.W * b.Dual.X + a.Real.X * b.Dual.W + a.Real.Y * b.Dual.Z - a.Real.Z * b.Dual.Y
				+ a.Dual.W * b.Real.X + a.Dual.X * b.Real.W + a.Dual.Y * b.Real.Z - a.Dual.Z * b.Real.Y,
				a.Real.W * b.Dual.Y - a.Real.X * b.Dual.Z + a.Real.Y * b.Dual.W + a.Real.Z * b.Dual.X
				+ a.Dual.W * b.Real.Y - a.Dual.X * b.Real.Z + a.Dual.Y * b.Real.W + a.Dual.Z * b.Real.X,
				a.Real.W * b.Dual.Z + a.Real.X * b.Dual.Y - a.Real.Y * b.Dual.X + a.Real.Z * b.Dual.W
				+ a.Dual.W * b.Real.Z + a.Dual.X * b.Real.Y - a.Dual.Y * b.Real.X + a.Dual.Z * b.Real.W,
				a.Real.W * b.Dual.W - a.Real.X * b.Dual.X - a.Real.Y * b.Dual.Y - a.Real.Z * b.Dual.Z
				+ a.Dual.W * b.Real.W - a.Dual.X * b.Real.X - a.Dual.Y * b.Real.Y - a.Dual.Z * b.Real.Z);
		}
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <returns></returns>
		public static DualQuaternion Multiply(DualQuaternion a, DualQuaternion b) {
			DualQuaternion result;
			Multiply(ref a, ref b, out result);
			return result;
		}
		/// <summary>
		/// 乗算
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <returns></returns>
		public static DualQuaternion operator *(DualQuaternion a, DualQuaternion b) {
			DualQuaternion result;
			Multiply(ref a, ref b, out result);
			return result;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 内積
		/// </summary>
		/// <param name="a"></param>
		/// <param name="b"></param>
		/// <param name="result"></param>
		public static void Dot(ref DualQuaternion a, ref DualQuaternion b, out DualQuaternion result) {
			DualQuaternion c;
			DualQuaternion.SwapConjugate(ref b, out c);
			DualQuaternion.Multiply(ref a, ref c, out result);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 長さの2乗（ノルム）。this * Conjugate(this)
		/// </summary>
		/// <param name="real">実部</param>
		/// <param name="dual">非実部</param>
		public void LengthSquared(out float real, out float dual) {
			real = Real.LengthSquared();
			_Quaternion.Dot(ref Real, ref Dual, out dual);
			dual += dual;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 長さの2乗の逆数
		/// </summary>
		/// <param name="real">実部</param>
		/// <param name="dual">非実部</param>
		public void ReciprocalLengthSquared(out float real, out float dual) {
			real = MathHelper.Rcp(Real.LengthSquared());
			Quaternion.Dot(ref Real, ref Dual, out dual);
			dual = -2 * dual * real * real;
			//real = 1 / a.Real.LengthSquared();
			//dual = -2 * (a.Real.X * a.Dual.X + a.Real.Y * a.Dual.Y + a.Real.Z * a.Dual.Z + a.Real.W * a.Dual.W) * real * real;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 正規化する。
		/// </summary>
		public void Normalize() {
			var r = MathHelper.Rcp(Real.Length());
			Quaternion a;
			float d;
			Quaternion.Dot(ref Real, ref Dual, out d);
			Quaternion.Multiply(ref Real, d * r * r, out a);
			Quaternion.Subtract(ref Dual, ref a, out Dual);
			Quaternion.Multiply(ref Dual, r, out Dual);
			Real.Normalize();
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 符号反転
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void Negate(ref DualQuaternion a, out DualQuaternion result) {
			Quaternion.Negate(ref a.Real, out result.Real);
			Quaternion.Negate(ref a.Dual, out result.Dual);
		}

		/// <summary>
		/// 符号反転
		/// </summary>
		/// <param name="a"></param>
		/// <returns></returns>
		public static DualQuaternion operator -(DualQuaternion a) {
			DualQuaternion result;
			Negate(ref a, out result);
			return result;
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 共役（乗算入れ替え用）
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void SwapConjugate(ref DualQuaternion a, out DualQuaternion result) {
			_Quaternion.Conjugate(ref a.Real, out result.Real);
			_Quaternion.Conjugate(ref a.Dual, out result.Dual);
		}

		/// <summary>
		/// 共役（座標変換用）
		/// </summary>
		/// <param name="a"></param>
		/// <param name="result"></param>
		public static void TransformConjugate(ref DualQuaternion a, out DualQuaternion result) {
			_Quaternion.Conjugate(ref a.Real, out result.Real);
			result.Dual = new Quaternion(a.Dual.X, a.Dual.Y, a.Dual.Z, -a.Dual.W);
		}
	}

	public partial struct DualQuaternion {
		/// <summary>
		/// 参照するデュアルクォータニオンとの実部のなす角度がπ/2以上の場合、
		/// 符号を反転した値を返す。そうでなければ、そのままの値を返す。
		/// </summary>
		/// <param name="reference"></param>
		/// <param name="result"></param>
		public void Regularize(ref DualQuaternion reference, out DualQuaternion result) {
			float cosine;
			Quaternion.Dot(ref Real, ref reference.Real, out cosine);
			if (cosine < 0.0f) {
				Negate(ref this, out result);
			} else {
				result = this;
			}
		}
	}

	public partial struct DualQuaternion {
		static readonly DualQuaternion identity = new DualQuaternion(0, 0, 0, 1, 0, 0, 0, 0);

		/// <summary>
		///	単位デュアルクォータニオン（乗法の単位元）
		/// </summary>
		public static DualQuaternion Identity { get { return identity; } }
	}

}